#include "mahonyAHRS.h"

extern SensorInfo_TypeDef sensor_info;



// Definitions

#define Kp 5.f   //2.0f                                            
																															/* 
                                                              * proportional gain governs rate of 
                                                              * convergence to accelerometer/magnetometer 
																															*/
#define Ki 0.025f                                             /* 
                                                              * integral gain governs rate of 
                                                              * convergence of gyroscope biases 
																															*/
//---------------------------------------------------------------------------------------------------
// Variable definitions
float x_min = 0,x_max = 0;
float y_min = 0,y_max = 0;
float z_min = 0,z_max = 0;
float x_calib = 0 , y_calib = 0 , z_calib = 0;
volatile uint32_t     last_update, now_update;               /* Sampling cycle count, ubit ms */
volatile float        exInt=0, eyInt=0, ezInt=0;                   /* error integral */
volatile float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f;


float invSqrt(float x) 
{
	float halfx = 0.5f * x;
	float y = x;
	long i = *(long*)&y;
	i = 0x5f3759df - (i>>1);
	y = *(float*)&i;
	y = y * (1.5f - (halfx * y * y));
	return y;
}


// asin
float safe_asin(float v)
{
	if (isnan(v)) {
		return 0.0f;
	}
	if (v >= 1.0f) {
		return PI/2;
	} 
	if (v <= -1.0f) {
		return -PI/2;  
	}
	return asin(v);
}

float gravity = 0;
void Attitude_MahonyAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz)
{
	float norm;
	float hx, hy, hz;
	float vx, vy, vz, wx, wy, wz;
	float ex=0, ey=0, ez=0, halfT;
	float tempq0,tempq1,tempq2,tempq3;
	float mx_tmp,my_tmp,mz_tmp;
	float bx, bz;
	
	float q0q0 = q0*q0;
	float q0q1 = q0*q1;
	float q0q2 = q0*q2;
	float q0q3 = q0*q3;
	float q1q1 = q1*q1;
	float q1q2 = q1*q2;
	float q1q3 = q1*q3;
	float q2q2 = q2*q2;   
	float q2q3 = q2*q3;
	float q3q3 = q3*q3;   

	
	int8_t ret;
	ret = AK09915_GetCalibration(&x_min,&x_max,&y_min,&y_max,&z_min,&z_max);  
	//ret = 1;
	
	if( ret != 0)
	{
		mx = 0;
		my = 0;
		mz = 0;
	}
	else
	{
		x_calib = (mx - (x_max + x_min)/2.f );
		y_calib = (my - (y_max + y_min)/2.f );
		z_calib = (mz - (z_max + z_min)/2.f );
	}

	now_update  = HAL_GetTick(); //ms
	halfT       = ((float)(now_update - last_update) / 2000.0f);
	last_update = now_update;

	if(ax == 0 && ay ==0 && az == 0)
	{
		return ;
	}

	gravity =  ( 2.0f*ax*(q1q3 - q0q2) + 2.0f*ay*(q2q3 + q0q1) + 2.0f*az*(0.5f - q1q1 - q2q2) );

	/* Fast inverse square-root */
	norm = invSqrt(ax*ax + ay*ay + az*az);       
	ax = ax * norm;
	ay = ay * norm;
	az = az * norm;

	if(mx == 0 && my ==0 && mz == 0)
	{
		ex = 0;
		ey = 0;
		ez = 0;
	}	
	else
	{
		norm = invSqrt(x_calib*x_calib + y_calib*y_calib + z_calib*z_calib);     
		mx_tmp = x_calib * norm;
		my_tmp = y_calib * norm;
		mz_tmp = z_calib * norm;
		
		/* compute reference direction of flux */
		hx = 2.0f*mx_tmp*(0.5f - q2q2 - q3q3) + 2.0f*my_tmp*(q1q2 - q0q3) + 2.0f*mz_tmp*(q1q3 + q0q2);
		hy = 2.0f*mx_tmp*(q1q2 + q0q3) + 2.0f*my_tmp*(0.5f - q1q1 - q3q3) + 2.0f*mz_tmp*(q2q3 - q0q1);
		hz = 2.0f*mx_tmp*(q1q3 - q0q2) + 2.0f*my_tmp*(q2q3 + q0q1) + 2.0f*mz_tmp*(0.5f - q1q1 - q2q2);    
		
		bx = sqrt((hx*hx) + (hy*hy));
		bz = hz; 	

		wx = 2.0f*bx*(0.5f - q2q2 - q3q3) + 2.0f*bz*(q1q3 - q0q2);
		wy = 2.0f*bx*(q1q2 - q0q3) + 2.0f*bz*(q0q1 + q2q3);
		wz = 2.0f*bx*(q0q2 + q1q3) + 2.0f*bz*(0.5f - q1q1 - q2q2);  

		ex = (my_tmp*wz - mz_tmp*wy);
		ey = (mz_tmp*wx - mx_tmp*wz);
		ez = (mx_tmp*wy - my_tmp*wx);		
	}


	
	/* estimated direction of gravity and flux (v and w) */
	vx = 2.0f*(q1q3 - q0q2);
	vy = 2.0f*(q0q1 + q2q3);
	vz = q0q0 - q1q1 - q2q2 + q3q3;

	/* 
	 * error is sum of cross product between reference direction 
	 * of fields and direction measured by sensors 
	 */
	ex += (ay*vz - az*vy);
	ey += (az*vx - ax*vz);
	ez += (ax*vy - ay*vx);

	/* PI */
	if(ex != 0.0f && ey != 0.0f && ez != 0.0f)
	{
		exInt = exInt + ex * Ki * halfT;
		eyInt = eyInt + ey * Ki * halfT;	
		ezInt = ezInt + ez * Ki * halfT;
		
		gx = gx + Kp*ex + exInt;
		gy = gy + Kp*ey + eyInt;
		gz = gz + Kp*ez + ezInt;
	}
	
	tempq0 = q0 + (-q1*gx - q2*gy - q3*gz) * halfT;
	tempq1 = q1 + (q0*gx + q2*gz - q3*gy) * halfT;
	tempq2 = q2 + (q0*gy - q1*gz + q3*gx) * halfT;
	tempq3 = q3 + (q0*gz + q1*gy - q2*gx) * halfT;  

	/* normalise quaternion */
	norm = invSqrt(tempq0*tempq0 + tempq1*tempq1 + tempq2*tempq2 + tempq3*tempq3);
	q0 = tempq0 * norm;
	q1 = tempq1 * norm;
	q2 = tempq2 * norm;
	q3 = tempq3 * norm;
	

	// euro angle function 
	// roll-X axis-gamma -- roll = arctan( (2q0q1+2q2q3)/(q0^2-q1^2-q2^2+q3^2))
	// pitch-Y axis-beta -- pitch = arcsin( 2q1q3 - 2q0q2 )
	// yaw-Z axis-alpha  -- yaw = arctan( (2q0q3+2q1q2)/q0^2+q1^2-q2^2-q3^2)
	
	sensor_info.euro_angle.roll = (atan2(2.0f*(q0q1 + q2q3),1 - 2.0f*(q1q1 + q2q2)))* 180/PI;
	sensor_info.euro_angle.pitch = safe_asin(2.0f*(q0q2 - q1q3))* 180/PI; 
	sensor_info.euro_angle.yaw = atan2(2.0f*q1q2 + 2.0f*q0q3, -2.0f*q2q2 - 2.0f*q3q3 + 1) * 180/PI; // yaw	
}


void MahonyAHRS_GetQuaternion(float *Q)
{
	Q[0] = q0;
	Q[1] = q1;
	Q[2] = q2;
	Q[3] = q3;
}
